#include "coordMath.h"

/*!@brief This will convert from meter based earth centered earth fixed (x, y, z)
 * to radian based lat lon height
 */

LLHvector ECEFtoLLH(ECEFvector coord) {

    double f = (C_WGS84_a - C_WGS84_b) / C_WGS84_a;
    double e = sqrt(2 * f - f * f);

    double h = 0;
    double N = C_WGS84_a;

    LLHvector result;

    result.lon = atan2(coord.y, coord.x); /* longitude */

    for(int n = 0; n < 50; ++n) {

        double sin_lat = coord.z / (N * (1 - e * e) + h);

        result.lat = atan((coord.z + e * e * N * sin_lat) / sqrt(coord.x * coord.x + coord.y * coord.y));

        N = C_WGS84_a / sqrt(1 - e * e * sin(result.lat) * sin(result.lat));
        h = sqrt(coord.x * coord.x + coord.y * coord.y) / cos(result.lat) - N;
    } /* end for */

    result.height = h;

    return result;
} /* end ECEFtoLLH */
